#include <string>
#include <iostream>
#include <fstream>
#include <sstream>

#include "VO_AAMShape2D.h"

using namespace std;


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      Copy constructor

 @param      iShape	The input shape

 @return     N/A

 */
VO_AAMShape2D::VO_AAMShape2D( const VO_AAMShape2D &iShape )
{
	this->CopyData (iShape);
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator= overloading, similar to copy constructor

 @param      iShape	The input shape

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator=(const VO_AAMShape2D &iShape)
{
	this->CopyData (iShape);
	return *this;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator= overloading, similar to copy constructor

 @param      value	assign all values in VO_AAMShape2D to value

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator=(float value)
{
	if (this->m_vPoint.size())
	{
		CvPoint2D32f tempCvPoint2D32f;
		for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
		{
			tempCvPoint2D32f.x = value;
			tempCvPoint2D32f.y = value;
			this->m_vPoint[i].SetXY(tempCvPoint2D32f);
		}
	}

	return *this;  
}

/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator+ overloading, shift one AAM shape by value

 @param      value

 @return     VO_AAMShape2D

 */
VO_AAMShape2D VO_AAMShape2D::operator+(float value)
{    
	VO_AAMShape2D res(*this);
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempAAMPoint2d;

	for (unsigned int i = 0; i < res.GetPoints().size(); i++)
	{
		tempCvPoint2D32f.x = res.GetPoint(i).GetXY().x + value;
		tempCvPoint2D32f.y = res.GetPoint(i).GetXY().y + value;
		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		res.SetPoint(tempAAMPoint2d, i);
	}

	return res;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator+= overloading, add value to this AAM shape 

 @param      value

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator+=(float value)
{
	CvPoint2D32f tempCvPoint2D32f;

	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x + value;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y + value;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	return *this;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator+ overloading, add two AAM shape to one 

 @param      iShape      the added AAM shape

 @return     VO_AAMShape2D

 */
VO_AAMShape2D VO_AAMShape2D::operator+(const VO_AAMShape2D &iShape)
{    
	VO_AAMShape2D res(*this);
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempAAMPoint2d;

	for (unsigned int i = 0; i < res.GetPoints().size(); i++)
	{
		tempCvPoint2D32f.x = res.GetPoint(i).GetXY().x + iShape.GetPoint(i).GetXY().x;
		tempCvPoint2D32f.y = res.GetPoint(i).GetXY().y + iShape.GetPoint(i).GetXY().y;
		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		res.SetPoint(tempAAMPoint2d, i);
	}

	return res;
}

/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator+= overloading, add the input AAM shape to this AAM shape

 @param      iShape      the added AAM shape

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator+=(const VO_AAMShape2D &iShape)
{
	CvPoint2D32f tempCvPoint2D32f;

	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x + iShape.GetPoint(i).GetXY().x;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y + iShape.GetPoint(i).GetXY().y;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	return *this;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator- overloading, shift one AAM shape by -value

 @param      value

 @return     VO_AAMShape2D

 */
VO_AAMShape2D VO_AAMShape2D::operator-(float value)
{    
	VO_AAMShape2D res(*this);
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempAAMPoint2d;

	for (unsigned int i = 0; i < res.GetPoints().size(); i++)
	{
		tempCvPoint2D32f.x = res.GetPoint(i).GetXY().x - value;
		tempCvPoint2D32f.y = res.GetPoint(i).GetXY().y - value;
		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		res.SetPoint(tempAAMPoint2d, i);
	}

	return res;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator-= overloading, subtract value from this AAM shape 

 @param      value

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator-=(float value)
{
	CvPoint2D32f tempCvPoint2D32f;

	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x - value;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y - value;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	return *this;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator- overloading, subtract one shape from another

 @param      iShape      the subtracted AAM shape

 @return     VO_AAMShape2D

 */
VO_AAMShape2D VO_AAMShape2D::operator-(const VO_AAMShape2D &iShape)
{
	VO_AAMShape2D res(*this);
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempAAMPoint2d;

	for (unsigned int i = 0; i < res.GetPoints().size(); i++)
	{
		tempCvPoint2D32f.x = res.GetPoint(i).GetXY().x - iShape.GetPoint(i).GetXY().x;
		tempCvPoint2D32f.y = res.GetPoint(i).GetXY().y - iShape.GetPoint(i).GetXY().y;
		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		res.SetPoint(tempAAMPoint2d, i);
	}

	return res;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator-= overloading, subtract the input AAM shape from this AAM shape

 @param      iShape      the subtracted AAM shape

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator-=(const VO_AAMShape2D &iShape)
{
	CvPoint2D32f tempCvPoint2D32f;

	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x - iShape.GetPoint(i).GetXY().x;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y - iShape.GetPoint(i).GetXY().y;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	return *this;
}

/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator* overloading, scale a shape with input float value

 @param      value      scale size

 @return     VO_AAMShape2D

 */
VO_AAMShape2D VO_AAMShape2D::operator*(float value)
{
	VO_AAMShape2D res(*this);
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempAAMPoint2d;

	for (unsigned int i = 0; i < res.GetPoints().size(); i++)
	{
		tempCvPoint2D32f.x = res.GetPoint(i).GetXY().x * value;
		tempCvPoint2D32f.y = res.GetPoint(i).GetXY().y * value;
		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		res.SetPoint(tempAAMPoint2d, i);
	}

	return res;
}

/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator*= overloading, scale this shape with input float value

 @param      iShape      the subtracted AAM shape

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator*=(float value)
{
	CvPoint2D32f tempCvPoint2D32f;

	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x * value;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y * value;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	return *this;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator* overloading, dot product of two AAM shapes

 @param      iShape      AAM shape to be dot producted

 @return     float       dot product

 */
float VO_AAMShape2D::operator*(const VO_AAMShape2D &iShape)
{
	float result = 0.;

	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		result += this->m_vPoint[i].GetXY().x * iShape.GetPoint(i).GetXY().x;
		result += this->m_vPoint[i].GetXY().y * iShape.GetPoint(i).GetXY().y;
	}

	return result;
}

/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator/ overloading, scale a shape

 @param      value      1.0/value = scale size

 @return     VO_AAMShape2D

 */
VO_AAMShape2D VO_AAMShape2D::operator/(float value)
{
	assert (value != 0);

	VO_AAMShape2D res(*this);
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempAAMPoint2d;

	for (unsigned int i = 0; i < res.GetPoints().size(); i++)
	{
		tempCvPoint2D32f.x = res.GetPoint(i).GetXY().x / value;
		tempCvPoint2D32f.y = res.GetPoint(i).GetXY().y / value;
		tempAAMPoint2d.SetXY(tempCvPoint2D32f);
		res.SetPoint(tempAAMPoint2d, i);
	}

	return res;
}

/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      operator/= overloading, scale this shape with input float value

 @param      value      1.0/value = the scaled value

 @return     VO_AAMShape2D&

 */
VO_AAMShape2D& VO_AAMShape2D::operator/=(float value)
{
	assert (value != 0);

	CvPoint2D32f tempCvPoint2D32f;

	for(unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{		
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x / value;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y / value;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	return *this;
}


/**

 @author     JIA Pei

 @version    2006-04-13

 @brief      copy all data from the input VO_AAMShape2D to this VO_AAMShape2D

 @param      iShape      1.0/value = the scaled value

 @return     void

 */
void VO_AAMShape2D::CopyData( const VO_AAMShape2D& iShape )
{
	// copy vector data (i.e. the point coordinates)
	this->m_vPoint.resize( iShape.GetSize() );
	this->m_vPoint                  = iShape.GetPoints();
	this->m_sHostImage              = iShape.GetHostImage();
}



/**

 @author     JIA Pei

 @version    2006-09-02

 @brief      Translate this shape.

 @param      x	X-translation.

 @param      y	Y-translation.

 @return     Nothing.

 */
void VO_AAMShape2D::Translate( float x, float y )
{
	CvPoint2D32f tempCvPoint2D32f;
	for(unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{		
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x + x;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y + y;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}
}



/**

 @author     JIA Pei

 @version    2006-09-02

 @brief		Scale this shape.

 @param      s			Scale factor

 @return     void

 */
void VO_AAMShape2D::Scale( float s)
{
	float x, y;
	CvPoint2D32f cog;

	// Get Centre of gravity first
	GetShapeCOG (cog);

	// Centre the shape about the origin
	Translate (-1*cog.x, -1*cog.y);

	CvPoint2D32f tempCvPoint2D32f;
	for(unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{		
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x * s;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y * s;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	// Translate the shape back
	Translate (cog.x, cog.y);
}


/**

 @author     JIA Pei

 @version    2006-09-02

 @brief		Scale this shape

 @param      sx			Scale factor in X direction

 @param      sy			Scale factor in Y direction

 @return     void

 */
void VO_AAMShape2D::ScaleXY( float sx, float sy)
{
	CvPoint2D32f tempCvPoint2D32f;
	for(unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		tempCvPoint2D32f.x = this->m_vPoint[i].GetXY().x * sx;
		tempCvPoint2D32f.y = this->m_vPoint[i].GetXY().y * sy;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}
}



/**

 @author     JIA Pei

 @version    2006-09-02

 @brief		Rotates the shape 'theta' radians

 @param      theta		Rotation angle in radians

 @return     Nothing.

 */
void VO_AAMShape2D::Rotate( float theta)
{
	float x, y;
	CvPoint2D32f cog;

	// Get Centre of gravity first
	GetShapeCOG (cog);

	// Centre the shape about the origin
	Translate (-1*cog.x, -1*cog.y);

	// set up rotation matrix
	float c00 =  cos( theta );
	float c01 = -sin( theta );
	float c10 =  sin( theta );
	float c11 =  cos( theta );

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{
		CvPoint2D32f tempCvPoint2D32f;
		x = this->m_vPoint[i].GetXY().x;
		y = this->m_vPoint[i].GetXY().y;
		tempCvPoint2D32f.x =  c00*x+c01*y;
		tempCvPoint2D32f.y =  c10*x+c11*y;
		this->m_vPoint[i].SetXY(tempCvPoint2D32f);
	}

	// Translate the shape back
	Translate (cog.x, cog.y);
}



/**

 @author	    JIA Pei

 @version	2006-09-02

 @brief		Normalize the shape by translating to its Center Of Gravity
 scaling by the reciprocal of the 2-norm

 @return	    The 2-norm of the shape after translating to origin
 */

float VO_AAMShape2D::Normalize()
{
	float x,y;

	this->CenterOfGravity( x, y );

	this->Translate( -x, -y );

	float norm = this->GetShapeNorm();

	this->Scale( 1.0f/norm );

	return norm;
}


/**

 @author	    DEENA Salil

 @version	2010-01-09

 @brief		Normalize the shape by translating to its Center Of Gravity
 scaling by the reciprocal of the 2-norm

 @return	    The 2-norm of the shape after translating to origin
 */

float VO_AAMShape2D::NormalizeNoScale()
{
	float x,y;

	this->CenterOfGravity( x, y );

	this->Translate( -x, -y );

	float norm = this->GetShapeNorm();

	//this->Scale( 1.0f/norm );

	return norm;
}



/**

 @author   JIA Pei

 @version  2006-04-13

 @brief    Returns the 2-norm of this centralized shape

 @return   The 2-norm of the shape after translating to origin

 */
float VO_AAMShape2D::GetShapeSize() const
{
	VO_AAMShape2D tmp(*this);

	return tmp.Normalize();
}


/**

 @author     JIA Pei

 @version    2006-09-02

 @brief      Get the norm of the shape

 @return     float

 */
float VO_AAMShape2D::GetShapeNorm() const
{
	float norm = 0.0;

	// Normalize the vector to unit length, using the 2-norm.
	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		norm += this->m_vPoint[i].GetXY().x * this->m_vPoint[i].GetXY().x;
		norm += this->m_vPoint[i].GetXY().y * this->m_vPoint[i].GetXY().y;
	}

	norm = sqrt(norm);

	return norm;
}



/**

 @author	    JIA Pei

 @version	2006-09-02

 @brief		Returns the rotation between ref and this (in radians).
 Get the rotation between two shapes by minimizing the sum of squared point distances, 
 as described by Goodall (and Bookstein) using Singular Value Decomposition (SVD).

 Note that both shapes must be normalized with respect to scale and position beforehand.
 This could be done by using VO_AAMShape2D::Normalize(). 

 @return	    The estimated angle, theta, between the two shapes.

 */
float VO_AAMShape2D::GetRotation( const VO_AAMShape2D &ref ) const
{
	assert( ref.m_vPoint.size() == this->m_vPoint.size() );

	CvMat* mRef            =   cvCreateMat ( (int)this->m_vPoint.size(), 2, CV_32FC1 );
	CvMat* mRefTranspose   =   cvCreateMat ( 2, (int)this->m_vPoint.size(), CV_32FC1 );
	CvMat* mS              =   cvCreateMat ( (int)this->m_vPoint.size(), 2, CV_32FC1 );
	CvMat* res             =   cvCreateMat ( 2, 2, CV_32FC1 );

	// get data as OpenCV matrices ( nbPoints x 2 columns )
	for(unsigned int i=0; i < this->m_vPoint.size(); i++)
	{
		CV_MAT_ELEM( *mRef, float, i, 0 ) = ref.GetPoint(i).GetXY().x;
		CV_MAT_ELEM( *mRef, float, i, 1 ) = ref.GetPoint(i).GetXY().y;

		CV_MAT_ELEM( *mS, float, i, 0 ) = this->m_vPoint[i].GetXY().x;
		CV_MAT_ELEM( *mS, float, i, 1 ) = this->m_vPoint[i].GetXY().y;
	}

	// Now, calculate the rotation

	cvTranspose( mRef, mRefTranspose );

	cvMatMul( mRefTranspose, mS, res );

	CvMat* S   =   cvCreateMat (2, 1, CV_32FC1);
	CvMat* UT  =   cvCreateMat (2, 2, CV_32FC1);
	CvMat* V   =   cvCreateMat (2, 2, CV_32FC1);

	// SVD Definition in OpenCV
	// A = U*W*V^T = U^T*W*V
	// cvSVD( CvArr* A, CvArr* W, CvArr* U=NULL, CvArr* V=NULL, int flags=0 );
	cvSVD( res, S, UT, V, CV_SVD_U_T );

	cvMatMul( V, UT, res );

	// res now holds a normal 2x2 rotation matrix
	float angle;
	float cos_theta = CV_MAT_ELEM( *res, float, 0, 0 );
	float sin_theta = CV_MAT_ELEM( *res, float, 1, 0 );

	// cos_theta should be less than 1.0; but here, cos_theta is calculated by matrix computation, 
	// in stead of by acos() function, so cos_theta might have some special values like cos_theta >= 1.0.
	if ( ( fabs(1.0-cos_theta) < FLT_EPSILON ) || cos_theta >= 1.0 )
	{
		// cos_theta = 1  =>  shapes are already aligned
		angle = 0;
		return angle;
	}
	if ( fabs(cos_theta) < FLT_EPSILON )
	{
		// cos_theta = 0  =>  90 degrees rotation
		return (float)(M_PI/2.0);
	}
	// cos_theta should be bigger than -1.0; but here, cos_theta is calculated by  matrix computation,
	// in stead of by acos() function, so cos_theta might have some special values like cos_theta <= -1.0.
	if ( ( fabs(1.0+cos_theta) < FLT_EPSILON ) || cos_theta <= -1.0 )
	{
		// cos_theta=-1  =>  180 degrees rotation
		angle = (float)M_PI;
	}
	else
	{
		// get the rotation in radians
		float a_cos = acos( cos_theta );
		float a_sin = asin( sin_theta );

		// http://en.wikipedia.org/wiki/Trigonometric_function
		if (a_sin<0)
		{		
			// lower half of the unit circle
			angle = -a_cos;
		}
		else
		{
			// upper half of the unit circle
			angle = a_cos;
		}
	}

	cvReleaseMat(&mRef);
	cvReleaseMat(&mRefTranspose);
	cvReleaseMat(&mS);
	cvReleaseMat(&res);

	cvReleaseMat(&S);
	cvReleaseMat(&UT);
	cvReleaseMat(&V);

	return angle;
}

/**

 @author	    DEENA Salil

 @version	2010-01-06

 @brief		Returns the rotation between ref and this (in radians) by using the convex hull of the shape only.
 Get the rotation between two shapes by minimizing the sum of squared point distances, 
 as described by Goodall (and Bookstein) using Singular Value Decomposition (SVD).

 Note that both shapes must be normalized with respect to scale and position beforehand.
 This could be done by using VO_AAMShape2D::Normalize(). 

 @return	    The estimated angle, theta, between the two shapes.

 */
float VO_AAMShape2D::GetRotationConvHull( const VO_AAMShape2D &ref ) const
{
	assert( ref.m_vPoint.size() == this->m_vPoint.size() );

	CvMat* mRef            =   cvCreateMat ( (int)this->m_vPoint.size(), 2, CV_32FC1 );
	CvMat* mRefPoint2D            =   cvCreateMat ( 1, (int)this->m_vPoint.size(), CV_32FC2 );
	CvMat* mRefTranspose   =   cvCreateMat ( 2, (int)this->m_vPoint.size(), CV_32FC1 );
	CvMat* mS              =   cvCreateMat ( (int)this->m_vPoint.size(), 2, CV_32FC1 );
	CvMat* mSPoint2D              =   cvCreateMat ( 1, (int)this->m_vPoint.size(), CV_32FC2 );
	CvMat* res             =   cvCreateMat ( 2, 2, CV_32FC1 );

	// get data as OpenCV matrices ( nbPoints x 2 columns )
	for(unsigned int i=0; i < this->m_vPoint.size(); i++)
	{
		CV_MAT_ELEM( *mRef, float, i, 0 ) = ref.GetPoint(i).GetXY().x;
		CV_MAT_ELEM( *mRef, float, i, 1 ) = ref.GetPoint(i).GetXY().y;

		CV_MAT_ELEM( *mRefPoint2D, CvPoint2D32f, 0, i ) = ref.GetPoint(i).GetXY();

		CV_MAT_ELEM( *mS, float, i, 0 ) = this->m_vPoint[i].GetXY().x;
		CV_MAT_ELEM( *mS, float, i, 1 ) = this->m_vPoint[i].GetXY().y;

		CV_MAT_ELEM( *mSPoint2D, CvPoint2D32f, 0, i ) = this->m_vPoint[i].GetXY();	
	}

	cvTranspose( mRef, mRefTranspose );

	CvMat* mRefConvexHullPoint2D = cvCreateMat (1, this->m_vPoint.size(), CV_32FC2);
	CvMat* mSConvexHullPoint2D = cvCreateMat (1, this->m_vPoint.size(), CV_32FC2);

	CvSeq *mRefHull, *mSHull;

	int m_iNumHullPoints_Ref;
	int m_iNumHullPoints_S;
	int m_iNumConvexHullPoints;

	mRefHull = cvConvexHull2(mRefPoint2D, mRefConvexHullPoint2D, CV_COUNTER_CLOCKWISE, 0 );
	mSHull = cvConvexHull2(mSPoint2D, mSConvexHullPoint2D, CV_COUNTER_CLOCKWISE, 0 );	

	m_iNumHullPoints_Ref = mRefConvexHullPoint2D->cols;
	m_iNumHullPoints_S = mSConvexHullPoint2D->cols;


	if (m_iNumHullPoints_Ref == m_iNumHullPoints_S || m_iNumHullPoints_Ref < m_iNumHullPoints_S)
		m_iNumConvexHullPoints = m_iNumHullPoints_Ref;
	else
	{
		m_iNumConvexHullPoints = m_iNumHullPoints_S;	
	} 

	if (m_iNumHullPoints_Ref != m_iNumHullPoints_S)
	{
		cout << "Inconsistencies in Convex Hull" << endl;
		exit(1);	
	}		


	// Now get the convex hull points into a float Matrix	
	CvMat* mRefConvexHull = cvCreateMat (m_iNumConvexHullPoints, 2, CV_32FC1);
	CvMat* mSConvexHull = cvCreateMat (m_iNumConvexHullPoints, 2, CV_32FC1);

	cout << m_iNumConvexHullPoints << endl;  

	// cout << m_iNumConvexHullPoints << endl;
	for(unsigned int i=0; i < m_iNumConvexHullPoints; i++)
	{
		CV_MAT_ELEM( *mRefConvexHull, float, i, 0 ) = CV_MAT_ELEM( *mRefConvexHullPoint2D, CvPoint2D32f, 0, i ).x;
		CV_MAT_ELEM( *mRefConvexHull, float, i, 1 ) = CV_MAT_ELEM( *mRefConvexHullPoint2D, CvPoint2D32f, 0, i ).y;

		CV_MAT_ELEM( *mSConvexHull, float, i, 0 ) = CV_MAT_ELEM( *mSConvexHullPoint2D, CvPoint2D32f, 0, i ).x;
		CV_MAT_ELEM( *mSConvexHull, float, i, 1 ) = CV_MAT_ELEM( *mSConvexHullPoint2D, CvPoint2D32f, 0, i ).y;

	}

	cout << "mRefConvexHull" << endl;	
	for(unsigned int i=0; i < m_iNumConvexHullPoints; i++)
	{	
		cout << CV_MAT_ELEM( *mRefConvexHull, float, i, 0 ) << " " << CV_MAT_ELEM( *mRefConvexHull, float, i, 1 ) << endl;	    
	}	

	cout << "mSConvexHull" << endl;	
	for(unsigned int i=0; i < m_iNumConvexHullPoints; i++)
	{	
		cout << CV_MAT_ELEM( *mSConvexHull, float, i, 0 ) << " " << CV_MAT_ELEM( *mSConvexHull, float, i, 1 ) << endl;	    
	}
	//exit(1);	

	CvMat* mRefConvexHullTranspose   =   cvCreateMat ( 2, m_iNumConvexHullPoints, CV_32FC1 );	

	// Now, calculate the rotation
	cvTranspose( mRefConvexHull, mRefConvexHullTranspose );	
	cvMatMul( mRefConvexHullTranspose, mSConvexHull, res );

	CvMat* S   =   cvCreateMat (2, 1, CV_32FC1);
	CvMat* UT  =   cvCreateMat (2, 2, CV_32FC1);
	CvMat* V   =   cvCreateMat (2, 2, CV_32FC1);

	// SVD Definition in OpenCV
	// A = U*W*V^T = U^T*W*V
	// cvSVD( CvArr* A, CvArr* W, CvArr* U=NULL, CvArr* V=NULL, int flags=0 );
	cvSVD( res, S, UT, V, CV_SVD_U_T );

	cvMatMul( V, UT, res );

	// res now holds a normal 2x2 rotation matrix
	float angle;
	float cos_theta = CV_MAT_ELEM( *res, float, 0, 0 );
	float sin_theta = CV_MAT_ELEM( *res, float, 1, 0 );

	// cos_theta should be less than 1.0; but here, cos_theta is calculated by matrix computation, 
	// in stead of by acos() function, so cos_theta might have some special values like cos_theta >= 1.0.
	if ( ( fabs(1.0-cos_theta) < FLT_EPSILON ) || cos_theta >= 1.0 )
	{
		// cos_theta = 1  =>  shapes are already aligned
		angle = 0;
		return angle;
	}
	if ( fabs(cos_theta) < FLT_EPSILON )
	{
		// cos_theta = 0  =>  90 degrees rotation
		return (float)(M_PI/2.0);
	}
	// cos_theta should be bigger than -1.0; but here, cos_theta is calculated by  matrix computation,
	// in stead of by acos() function, so cos_theta might have some special values like cos_theta <= -1.0.
	if ( ( fabs(1.0+cos_theta) < FLT_EPSILON ) || cos_theta <= -1.0 )
	{
		// cos_theta=-1  =>  180 degrees rotation
		angle = (float)M_PI;
	}
	else
	{
		// get the rotation in radians
		float a_cos = acos( cos_theta );
		float a_sin = asin( sin_theta );

		// http://en.wikipedia.org/wiki/Trigonometric_function
		if (a_sin<0)
		{		
			// lower half of the unit circle
			angle = -a_cos;
		}
		else
		{
			// upper half of the unit circle
			angle = a_cos;
		}
	}

	cvReleaseMat(&mRef);
	cvReleaseMat(&mRefTranspose);
	cvReleaseMat(&mS);
	cvReleaseMat(&res);

	cvReleaseMat(&S);
	cvReleaseMat(&UT);
	cvReleaseMat(&V);

	free(mRefHull);
	free(mSHull);

	cvReleaseMat(&mRefConvexHull);
	cvReleaseMat(&mSConvexHull);

	cvReleaseMat(&mRefConvexHullTranspose);
	cvReleaseMat(&mRefConvexHullPoint2D);	
	cvReleaseMat(&mSConvexHullPoint2D);		

	//cout << "So far so good" << endl;
	return angle;
}

/**

 @author	    JIA Pei

 @version	2006-09-02

 @brief		Aligns this to 'ref' with respect to pose.

 @param	    ref			The reference shape.

 @param	    pTheta		Optional pointer to return the rotation carried out on this shape

 @return	    none.
 */
void VO_AAMShape2D::AlignTo( const VO_AAMShape2D &ref, float *pTheta )
{
	// make a copy of 'ref'
	VO_AAMShape2D refCpy( ref );
	float x,y;

	// translate, move this and refCpy to origin
	this->CenterOfGravity( x, y );
	this->Translate( -x, -y );
	refCpy.CenterOfGravity( x, y );
	refCpy.Translate( -x, -y ); // remember now, (x, y) are for ref (refCpy) !!

	// scale, using the 2-norm
	float this_size = this->GetShapeNorm();
	float ref_size = refCpy.GetShapeNorm();
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// Bypass the scaling. Need to add a parameter for this -- Salil 08/01/10
	this->Scale( ref_size/this_size );
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// align rotation between this and refCpy
	float theta;
	theta = -this->GetRotation( refCpy );
	this->Rotate( theta );

	if (pTheta)
	{
		*pTheta = -theta;
	}

	// translate this to ref origin
	this->Translate( x, y );

}

/**


 @author	    DEENA Salil

 @version	2010-12-21


 @brief		Aligns this to 'ref' with respect to pose without rotation.

 @param	    ref			The reference shape.


 @return	    none.
 */
void VO_AAMShape2D::AlignToNoRotation( const VO_AAMShape2D &ref )
{
	// make a copy of 'ref'
	VO_AAMShape2D refCpy( ref );
	float x,y;

	// translate, move this and refCpy to origin
	this->CenterOfGravity( x, y );
	this->Translate( -x, -y );
	refCpy.CenterOfGravity( x, y );
	refCpy.Translate( -x, -y ); // remember now, (x, y) are for ref (refCpy) !!

	// scale, using the 2-norm
	float this_size = this->GetShapeNorm();
	float ref_size = refCpy.GetShapeNorm();
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// Bypass the scaling. Need to add a parameter for this -- Salil 08/01/10
	this->Scale( ref_size/this_size );
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// align rotation between this and refCpy
	float theta;
	theta = -this->GetRotation( refCpy );

	/*this->Rotate( theta );

	if (pTheta)
	{
		*pTheta = -theta;
}*/

	// translate this to ref origin
	this->Translate( x, y );

}

/**


 @author	    DEENA Salil

 @version	2010-12-20

 @brief		Aligns this to 'ref' with respect to pose.


 @param	    ref			The reference shape.

 @param	    pTheta		Optional pointer to return the rotation carried out on this shape


 @return	    none.
 */
void VO_AAMShape2D::AlignToScaleXY( const VO_AAMShape2D &ref, float *pTheta )
{
	// make a copy of 'ref'
	VO_AAMShape2D refCpy( ref );
	float x,y;

	// translate, move this and refCpy to origin
	this->CenterOfGravity( x, y );
	this->Translate( -x, -y );
	refCpy.CenterOfGravity( x, y );
	refCpy.Translate( -x, -y ); // remember now, (x, y) are for ref (refCpy) !!

	// scale, using the 2-norm
	float this_width = this->GetWidth();
	float ref_width = refCpy.GetWidth();

	float this_height = this->GetHeight();
	float ref_height = refCpy.GetHeight();
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// Bypass the scaling. Need to add a parameter for this -- Salil 08/01/10
	this->ScaleXY( ref_width/this_width, ref_height/this_height );
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// align rotation between this and refCpy
	float theta;
	theta = -this->GetRotation( refCpy );
	this->Rotate( theta );

	if (pTheta)
	{
		*pTheta = -theta;
	}

	// translate this to ref origin
	this->Translate( x, y );

}

/**


 @author	    DEENA Salil

 @version	2010-12-21

 @brief		Aligns this to 'ref' with respect to pose without rotation.


 @param	    ref			The reference shape.

 @param	    pTheta		Optional pointer to return the rotation carried out on this shape


 @return	    none.
 */
void VO_AAMShape2D::AlignToScaleXYNoRotation( const VO_AAMShape2D &ref )
{
	// make a copy of 'ref'
	VO_AAMShape2D refCpy( ref );
	float x,y;

	// translate, move this and refCpy to origin
	this->CenterOfGravity( x, y );
	this->Translate( -x, -y );
	refCpy.CenterOfGravity( x, y );
	refCpy.Translate( -x, -y ); // remember now, (x, y) are for ref (refCpy) !!

	// scale, using the 2-norm
	float this_width = this->GetWidth();
	float ref_width = refCpy.GetWidth();

	float this_height = this->GetHeight();
	float ref_height = refCpy.GetHeight();
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// Bypass the scaling. Need to add a parameter for this -- Salil 08/01/10
	this->ScaleXY( ref_width/this_width, ref_height/this_height );
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	/*    // align rotation between this and refCpy
	 float theta;
	 theta = -this->GetRotation( refCpy );
	 this->Rotate( theta );

	 if (pTheta)
	 {
		 *pTheta = -theta;
}
*/

	// translate this to ref origin
	this->Translate( x, y );

}



/**

 @author	    DEENA Salil

 @version	2010-01-08

 @brief		Aligns this to 'ref' with respect to pose without scaling.

 @param	    ref			The reference shape.

 @param	    pTheta		Optional pointer to return the rotation carried out on this shape

 @return	    none.
 */
void VO_AAMShape2D::AlignToNoScale( const VO_AAMShape2D &ref, float *pTheta )
{
	// make a copy of 'ref'
	VO_AAMShape2D refCpy( ref );
	float x,y;

	// translate, move this and refCpy to origin
	this->CenterOfGravity( x, y );
	this->Translate( -x, -y );
	refCpy.CenterOfGravity( x, y );
	refCpy.Translate( -x, -y ); // remember now, (x, y) are for ref (refCpy) !!

	// scale, using the 2-norm
	float this_size = this->GetShapeNorm();
	float ref_size = refCpy.GetShapeNorm();
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// Bypass the scaling. Need to add a parameter for this -- Salil 08/01/10
	//this->Scale( ref_size/this_size );
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// align rotation between this and refCpy
	float theta;
	theta = -this->GetRotation( refCpy );
	this->Rotate( theta );

	if (pTheta)
	{
		*pTheta = -theta;
	}

	// translate this to ref origin
	this->Translate( x, y );

}

/**

 @author	    DEENA Salil

 @version	2010-07-14

 @brief		Aligns this to 'ref' with respect to pose without scaling.

 @param	    ref			The reference shape.

 @param	    pTheta		Optional pointer to return the rotation carried out on this shape

 @return	    t		        Returns the x and y Displacement carried out on this shape

 */
CvPoint2D32f VO_AAMShape2D::AlignToNoScale2( const VO_AAMShape2D &ref, float *pTheta, float *pNorm )
{
	// make a copy of 'ref'
	VO_AAMShape2D refCpy( ref );
	float xRef,yRef,xShape,yShape;
	CvPoint2D32f translation;	

	// translate, move this and refCpy to origin
	this->CenterOfGravity( xShape, yShape );
	this->Translate( -xShape, -yShape );
	refCpy.CenterOfGravity( xRef, yRef );
	refCpy.Translate( -xRef, -yRef ); // remember now, (x, y) are for ref (refCpy) !!

	translation.x = xShape;
	translation.y = yShape;	

	// scale, using the 2-norm
	float this_size = this->GetShapeNorm();
	float ref_size = refCpy.GetShapeNorm();
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// Bypass the scaling. Need to add a parameter for this -- Salil 08/01/10
	//this->Scale( ref_size/this_size );
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// align rotation between this and refCpy
	float theta;
	theta = -this->GetRotation( refCpy );
	this->Rotate( theta );

	if (pTheta)
	{
		*pTheta = -theta;
	}

	if (pNorm)
	{
		*pNorm = this_size;
	}	

	// translate this to ref origin
	this->Translate( xRef, yRef );

	return translation;
}


/**

 @author	    DEENA Salil

 @version	2010-01-06

 @brief		Aligns this to 'ref' with respect to pose using the convex hull only

 @param	    ref			The reference shape.

 @param	    pTheta		Optional pointer to return the rotation carried out on this shape

 @return	    none.
 */
void VO_AAMShape2D::AlignToConvHull( const VO_AAMShape2D &ref, float *pTheta )
{
	// make a copy of 'ref'
	VO_AAMShape2D refCpy( ref );
	float x,y;

	// translate, move this and refCpy to origin
	this->CenterOfGravity( x, y );
	this->Translate( -x, -y );
	refCpy.CenterOfGravity( x, y );
	refCpy.Translate( -x, -y ); // remember now, (x, y) are for ref (refCpy) !!

	// scale, using the 2-norm
	float this_size = this->GetShapeNorm();
	float ref_size = refCpy.GetShapeNorm();
	//cout << "Shape scale factor = " << ref_size/this_size << endl;
	this->Scale( ref_size/this_size );
	//cout << "Shape scale factor = " << ref_size/this_size << endl;

	// align rotation between this and refCpy
	float theta;
	theta = -this->GetRotationConvHull( refCpy );
	//theta = -this->GetRotation( refCpy ); 
	cout << theta << endl;
	this->Rotate( theta );

	if (pTheta)
	{
		*pTheta = -theta;
	}

	// translate this to ref origin
	this->Translate( x, y );

}



/**

 @author	    JIA Pei

 @version	2006-10-06

 @brief		Returns the transformation that aligns this to 'ref'

 @param	    ref	    input parameter - The reference shape

 @param	    scale	output parameter - scale value

 @param	    theta	output parameter - rotation angle theta

 @param	    t	    output parameter - translation in both X, Y directions

 @return	    void

 */
void VO_AAMShape2D::AlignTransformation( const VO_AAMShape2D &ref, float &scale, float &theta, CvPoint2D32f &t )
{
	VO_AAMShape2D refCpy( ref );
	VO_AAMShape2D thisCpy( *this );		// a must, we don't want to change "this" right now
	float x1,y1, x2, y2;

	// move thisCpy and refCpy to origin
	thisCpy.CenterOfGravity( x1, y1 );
	thisCpy.Translate( -x1, -y1 );
	refCpy.CenterOfGravity( x2, y2 );
	refCpy.Translate( -x2, -y2 );
	t.x = x2 - x1;
	t.y = y2 - y1;

	// normalize scale, using the 2-norm
	float this_size = thisCpy.GetShapeNorm();
	float ref_size = refCpy.GetShapeNorm();
	scale = ref_size/this_size;
	thisCpy.Scale( scale );	

	// align rotation between thisCpy and refCpy
	theta = -thisCpy.GetRotation( refCpy );	
}


/**

 @author	    JIA Pei

 @version	2008-02-25

 @brief		Proscrustes Analysis

 @param	    ref	    input parameter - The reference shape

 @param	    scale	output parameter - scale value

 @param	    theta	output parameter - rotation angle theta

 @param	    t	    output parameter - translation in both X, Y directions

 @return	    void

 @note       http://en.wikipedia.org/wiki/Procrustes_analysis

 */
void VO_AAMShape2D::ProcrustesAnalysis( const VO_AAMShape2D &ref, float &norm, float &theta, CvPoint2D32f &t )
{
	// move this to origin
	this->CenterOfGravity( t.x, t.y );
	this->Translate( -t.x, -t.y );

	// 2-norm = 1
	norm = this->GetShapeNorm();
	this->Scale( 1.0f/norm );

	// align rotation between this and ref
	theta = -this->GetRotation( ref );
	this->Rotate( theta );
}

/**

 @author	    JIA Pei

 @version	2008-02-25

 @brief	    Inverse Proscrustes Analysis

 @param	    scale	input parameter - scale value

 @param	    theta	input parameter - rotation angle theta

 @param	    t	    input parameter - translation in both X, Y directions

 @return	    void

 @note       http://en.wikipedia.org/wiki/Procrustes_analysis

 */
void VO_AAMShape2D::InverseProcrustesAnalysis( float scale, float theta, CvPoint2D32f t )
{
	// 2-norm back
	this->Scale(scale);

	// rotation back
	this->Rotate(theta);

	// COG back
	this->Translate(t.x, t.y);
}


/**

 @author	    JIA Pei

 @version	2006-09-02

 @brief		Calculate min value in X direction

 @return	    min value in X direction

 */
float VO_AAMShape2D::MinX() const
{	
	float val, min = FLT_MAX_10_EXP;

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{	
		val = this->m_vPoint[i].GetXY().x;
		min = val<min ? val : min;
	}
	return min;
}

/**

 @author	    JIA Pei

 @version	2006-09-02

 @brief		Calculate min value in Y direction

 @return	    min value in Y direction

 */
float VO_AAMShape2D::MinY() const
{	
	float val, min = FLT_MAX_10_EXP;

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{	
		val = this->m_vPoint[i].GetXY().y;
		min = val<min ? val : min;
	}
	return min;
}

/**

 @author	    JIA Pei

 @version	2006-09-02

 @brief		Calculate max value in X direction

 @return	    max value in X direction

 */
float VO_AAMShape2D::MaxX() const
{	
	float val, max = FLT_MIN_10_EXP ;

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{
		val = this->m_vPoint[i].GetXY().x;
		max = val>max ? val : max;
	}
	return max;
}


/**

 @author	    JIA Pei

 @version	2006-09-02

 @brief		Calculate max value in Y direction

 @return	    max value in Y direction

 */
float VO_AAMShape2D::MaxY() const
{	
	float val, max = FLT_MIN_10_EXP ;

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{
		val = this->m_vPoint[i].GetXY().y;
		max = val>max ? val : max;
	}
	return max;
}

/**

 @author	    DEENA Salil

 @version	2010-07-08

 @brief		Returns index of min value in X direction

 @return	    min value in X direction

 */
int VO_AAMShape2D::MinIndX() const
{	
	float val, min = FLT_MAX_10_EXP;
	int ind = 0;

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{	
		val = this->m_vPoint[i].GetXY().x;

		if(val < min)
		{
			ind = i;
			min = val;	
		}
	}
	return ind;
}

/**

 @author	    DEENA Salil

 @version	2010-07-08

 @brief		Returns index of min value in Y direction

 @return	    min value in Y direction

 */
int VO_AAMShape2D::MinIndY() const
{	
	float val, min = FLT_MAX_10_EXP;
	int ind = 0;

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{	
		val = this->m_vPoint[i].GetXY().y;

		if(val < min)
		{
			ind = i;
			min = val;		
		}
	}
	return ind;
}

/**

 @author	    DEENA Salil

 @version	2010-07-08

 @brief		Returns index of max value in X direction

 @return	    max value in X direction

 */
int VO_AAMShape2D::MaxIndX() const
{	
	float val, max = FLT_MIN_10_EXP ;
	int ind = 0;

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{
		val = this->m_vPoint[i].GetXY().x;
		if(val > max)
		{
			ind = i;
			max = val;	
		}
	}
	return ind;
}


/**

 @author	    DEENA Salil

 @version	2010-07-08

 @brief		Returns index of max value in Y direction

 @return	    max value in Y direction

 */
int VO_AAMShape2D::MaxIndY() const
{	
	float val, max = FLT_MIN_10_EXP ;
	int ind = 0;	

	for(unsigned int i=0;i<this->m_vPoint.size();i++)
	{
		val = this->m_vPoint[i].GetXY().y;
		if(val > max)
		{
			ind = i;
			max = val;	
		}
	}
	return ind;
}




/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Format change - from CvMat* to VO_AAMTriangle2D

 */
VO_AAMShape2D VO_AAMShape2D::VO_CvMat2AAMShape2D(const CvMat* iShape)
{
	unsigned int NbOfPoints = iShape->cols/2;
	VO_AAMShape2D resShape;
	resShape.SetSize(NbOfPoints);
	CvPoint2D32f tempCvPoint2D32f;
	VO_AAMPoint2D tempPoint2d;
	for(unsigned int i = 0; i < NbOfPoints; i++)
	{
		tempCvPoint2D32f.x = CV_MAT_ELEM( *iShape, float, 0, 2*i);
		tempCvPoint2D32f.y = CV_MAT_ELEM( *iShape, float, 0, 2*i+1);
		tempPoint2d.SetXY(tempCvPoint2D32f);
		resShape.SetPoint(tempPoint2d, i);
	}

	return resShape;
}


/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Format change - from VO_AAMTriangle2D to CvMat*

 */
void VO_AAMShape2D::VO_AAMShape2D2CvMat(CvMat* res) const
{
	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		CV_MAT_ELEM( *res, float, 0 , 2 * i) = this->m_vPoint[i].GetXY().x;
		CV_MAT_ELEM( *res, float, 0 , 2 * i + 1) = this->m_vPoint[i].GetXY().y;
	}
}

/**

 @author     DEENA Salil

 @version    2009-10-05

 @brief      Format change - from VO_AAMTriangle2D to CvMat* - In 2D format

 */
void VO_AAMShape2D::VO_AAMShape2D2CvMat2D(CvMat* res) const
{
	for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
	{
		CV_MAT_ELEM( *res, float, 0 , i) = this->m_vPoint[i].GetXY().x;
		CV_MAT_ELEM( *res, float, 1 , i) = this->m_vPoint[i].GetXY().y;
	}
}

/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Read a file and obtain all annotation data in VO_AAMShape2D

 @param      filename        input parameter, which .asf annotation file to read

 @param      VO_AAMShape2D   output parameter, save annotation data to AAM shape data structure

 */
void VO_AAMShape2D::ReadASF( const string &filename, VO_AAMShape2D& oAAMShape )
{
	fstream fp;
	fp.open(filename.c_str (), ios::in);

	stringstream ss;
	string temp;

	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );

	unsigned int NbOfPoints = atoi(temp.c_str ());

	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );
	getline( fp, temp );

	VO_AAMPoint2D temppoint;
	CvPoint2D32f tp;
	int tempInt;
	double tempFloat;	

	for (unsigned int i = 0; i < NbOfPoints; i++)
	{        
		getline(fp, temp, ' ');
		ss << temp;
		ss >> tempInt;
		ss.clear();
		temppoint.SetPath( tempInt );
		getline(fp, temp, ' ');
		ss << temp;
		ss >> tempInt;
		temppoint.SetType( tempInt );
		ss.clear();
		getline(fp, temp, ' ');
		// In DTU IMM , x means rows from left to right
		ss << temp;
		ss >> tp.x;
		ss.clear();
		getline(fp, temp, ' ');
		// In DTU IMM , y means cols from top to bottom
		ss << temp;
		ss >> tp.y;
		ss.clear();
		temppoint.SetXY(tp);
		getline(fp, temp, ' ');
		ss << temp;
		ss >> tempInt;
		temppoint.SetFrom( tempInt );
		ss.clear();
		getline(fp, temp, ' ');
		ss << temp;
		ss >> tempInt;
		temppoint.SetIndex( tempInt );
		ss.clear();
		getline(fp, temp);
		ss << temp;
		ss >> tempInt;
		temppoint.SetTo( tempInt );

		// cout << tp.x << " " << tp.y << endl; 

		ss.clear();
		getline(fp, temp, ' ');
		ss << temp;
		ss >> tempFloat;

		ss.clear();
		getline(fp, temp, ' ');
		ss << temp;
		ss >> tempFloat;

		ss.clear();
		getline(fp, temp, ' ');
		ss << temp;
		ss >> tempFloat;

		ss.clear();
		// In sum, topleft is (0,0), right bottom is (640,480)
		oAAMShape.InsertPoint(temppoint);
	}

	getline(fp, temp);
	getline(fp, temp);
	getline(fp, temp);
	getline(fp, temp);
	getline(fp, temp);

	oAAMShape.SetHostImage( temp );

	fp.close ();

}

/**

 @author     DEENA Salil

 @version    2009-10-05

 @brief      Write an asf file from annotation data in VO_AAMShape2D

 @param      filename        input parameter, which .asf annotation file to read

 */
void VO_AAMShape2D::WriteASF( const string &filename, const string &img_filename, int image_width, int image_height )
{
	FILE *fh;
	vector<VO_AAMPoint2D> points;
	int m_iNbPoints;

	fh = fopen( filename.c_str(), "wb" );

	if (!fh ) 
	{
		cout << "Invalid ASF filename ... exiting" << endl;
		exit(1);
	}

	/* 	// convert to relative coordinates
	 * 	if ( IsAbs() ) {
		 * 		
			 * 		Abs2Rel( image_width, image_height );		
			 * 	}
*/

	time_t mytime = time(0);

	points = this->GetPoints();
	m_iNbPoints = points.size();
	string currentTime = asctime(localtime(&mytime));
	RemoveCarriageReturn (currentTime);

	// write header
	//CTime t = CTime::GetCurrentTime();
	//string s = t.Format( "%A %B %d - %Y [%H:%M]" );
	fprintf( fh, "######################################################################\n" );
	fprintf( fh, "#\n#    AAM Shape File  -  written: %s\n#\n",   currentTime.c_str());
	fprintf( fh, "######################################################################\n\n" );

	// write nb points
	fprintf( fh, "#\n" );
	fprintf( fh, "# number of model points\n#\n%i\n\n", m_iNbPoints );	

	// write points
	fprintf( fh, "#\n" );
	fprintf( fh, "# model points\n" );
	fprintf( fh, "#\n" );
	fprintf( fh, "# format: <path#> <type> <x rel.> <y rel.> <point#> <connects from> <connects to> <user1> <user2> <user3>\n", m_iNbPoints );
	fprintf( fh, "#\n" );

	// check for user fields
	bool u1 = 0.0f;
	bool u2 = 0.0f;
	bool u3 = 0.0f;

	for(int i=0;i<m_iNbPoints;i++) {

		VO_AAMPoint2D p = this->GetPoint( i );
		fprintf( fh, "%i \t%i \t%.8f \t%.8f \t%i \t%i \t%i",
		p.GetPath(),
		p.GetType(),
		p.GetXY().x / image_width,
		p.GetXY().y / image_height,
		i,
		p.GetFrom(),
		p.GetTo()			);

		// print user fields if present
		fprintf( fh, "\t0.00" ); 
		fprintf( fh, "\t0.00" ); 
		fprintf( fh, "\t0.00" );         
		fprintf( fh, "\n");
	}

	// write host image
	fprintf( fh, "\n#\n" );
	fprintf( fh, "# host image\n" );
	fprintf( fh, "#\n" );
	fprintf( fh, "%s\n", img_filename.c_str() );

	// close file
	fclose( fh );
}

/**

 @author     JIA Pei

 @version    2008-03-04

 @brief      Read a file and obtain all annotation data in VO_AAMShape2D

 @param      filename        input parameter, which .pts annotation file to read

 @param      VO_AAMShape2D   output parameter, save annotation data to AAM shape data structure

 */
void VO_AAMShape2D::ReadPTS( const string &filename, VO_AAMShape2D& oAAMShape)
{
	fstream fp;
	fp.open(filename.c_str (), ios::in);

	string temp;
	stringstream ss;
	string::size_type loc;

	do
	{
		getline(fp, temp );
		loc = temp.find("n_points: 68", 0);
	}while ( loc == string::npos && !fp.eof( ) );
	unsigned int NbOfPoints = 68;
	getline(fp, temp);

	VO_AAMPoint2D temppoint;
	CvPoint2D32f tp;

	for (unsigned int i = 0; i < NbOfPoints; i++)
	{		
		getline(fp, temp, ' ');
		// x refers to a row from left to right
		ss << temp;
		ss >> tp.x;
		ss.clear();
		getline(fp, temp);        
		// x refers to a row from left to right
		ss << temp;
		ss >> tp.y;
		ss.clear();
		// In sum, topleft is (0,0), right bottom is (720,576)
		temppoint.SetXY(tp);
		oAAMShape.InsertPoint(temppoint);
	}

	fp.close ();
}

/**

 @author     DEENA Salil

 @version    2009-10-07

 @brief      Calculate the centre of gravity of the shape

 @param      cog        input/output parameter, containing the shape COG


 */
void  VO_AAMShape2D::GetShapeCOG (CvPoint2D32f &cog)
{
	int num_points = this->m_vPoint.size();
	float x = 0.0f;
	float y = 0.0f;

	for (int i=0; i < num_points; i++)
	{
		x = x + this->m_vPoint[i].GetXY().x;
		y = y + this->m_vPoint[i].GetXY().y;
	}

	cog.x = x / num_points;
	cog.y = y / num_points;	
}


void	VO_AAMShape2D::CopyShapeParams (VO_AAMShape2D& inputShape)
{
	for (int i = 0; i < this->m_vPoint.size(); i++)
	{
		this->m_vPoint[i].SetPath (inputShape.m_vPoint[i].GetPath());
		this->m_vPoint[i].SetType (inputShape.m_vPoint[i].GetType());
		this->m_vPoint[i].SetIndex (inputShape.m_vPoint[i].GetIndex());
		this->m_vPoint[i].SetTo (inputShape.m_vPoint[i].GetTo());
		this->m_vPoint[i].SetFrom (inputShape.m_vPoint[i].GetFrom());
	}
}


/**

 @author     DEENA Salil

 @version    2010-01-11

 @brief      Align two vertices of a shape such that they form a horizontal line

 @param      shapeIndex1  Index of first shape parameter

 @param      shapeIndex2  Index of second shape parameter

 */
void	VO_AAMShape2D::AlignPointsHorizontal (int shapeIndex1, int shapeIndex2)
{ 
	VO_AAMPoint2D left_vertex = this->GetPoint(shapeIndex1);
	VO_AAMPoint2D right_vertex = this->GetPoint(shapeIndex2);

	double tan_angle_of_tilt = (right_vertex.GetXY().y - left_vertex.GetXY().y) / (right_vertex.GetXY().x - left_vertex.GetXY().x);
	double angle_of_tilt = atan(tan_angle_of_tilt);

	this->Rotate(-1 * angle_of_tilt);	
}

/**

 @author     DEENA Salil

 @version    2010-01-11

 @brief      Translate a given vertex of a shape to a new position

 @param      shapeIndex  Index of vertex to translate

 @param      pos_x  The x position to translate to

 @param      pos_y  The y position to translate to

 */
void	VO_AAMShape2D::TranslateVertexToPosition (int shapeIndex, float pos_x, float pos_y)
{ 
	VO_AAMPoint2D left_vertex = this->GetPoint(shapeIndex);
	CvPoint2D32f translation;
	translation = left_vertex.GetXY();
	translation.x *= -1 + pos_x;
	translation.y *= -1 + pos_y;

	this->Translate (translation.x, translation.y); 
}

/**

 @author     DEENA Salil

 @version    2010-07-08

 @brief      Translate a given vertex of a shape to a new position given by the Y coordinates only

 @param      shapeIndex  Index of vertex to translate

 @param      pos_y  The y position to translate to

 */
void	VO_AAMShape2D::TranslateVertexToPositionY (int shapeIndex, float pos_y)
{ 
	VO_AAMPoint2D left_vertex = this->GetPoint(shapeIndex);
	CvPoint2D32f translation;
	translation = left_vertex.GetXY();
	translation.y *= -1 + pos_y;

	this->Translate (0, translation.y); 
}

/**

 @author     DEENA Salil

 @version    2010-07-08

 @brief      Translate a given vertex of a shape to a new position given by the X coordinates only

 @param      shapeIndex  Index of vertex to translate

 @param      pos_x  The x position to translate to

 */
void	VO_AAMShape2D::TranslateVertexToPositionX (int shapeIndex, float pos_x)
{ 
	VO_AAMPoint2D left_vertex = this->GetPoint(shapeIndex);
	CvPoint2D32f translation;
	translation = left_vertex.GetXY();
	translation.x *= -1 + pos_x;

	this->Translate (translation.x, 0); 
}
